import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
import cv2
import numpy as np
from rclpy.qos import QoSProfile

class ImagePublisher(Node):
    def __init__(self):
        super().__init__('image_publisher')
        qos_profile = QoSProfile(depth=10)
        self.publisher_ = self.create_publisher(Image, 'image_topic', qos_profile)
        self.timer = self.create_timer(0.5, self.publish_image)
        self.count = 0
        
        self.get_logger().info("The image publisher has started")

    def publish_image(self):
        image = np.zeros((480, 640, 3), dtype=np.uint8)
        image[:] = [50, 50, 50]
        text = f"ROS2 Image {self.count}"
        cv2.putText(image, text, (100, 240), 
                   cv2.FONT_HERSHEY_SIMPLEX, 1.0, (0, 255, 255), 2)
        
        radius = 50
        x = 320 + int(100 * np.sin(self.count * 0.1))
        y = 240 + int(100 * np.cos(self.count * 0.1))
        cv2.circle(image, (x, y), radius, (255, 0, 0), -1)
        
        msg = Image()
        msg.header.stamp = self.get_clock().now().to_msg()
        msg.header.frame_id = "camera_frame"
        msg.height = image.shape[0]
        msg.width = image.shape[1]
        msg.encoding = 'bgr8'
        msg.is_bigendian = False
        msg.step = image.shape[1] * 3
        msg.data = image.tobytes()
        
        self.publisher_.publish(msg)
        self.count += 1

def main(args=None):
    rclpy.init(args=args)
    publisher = ImagePublisher()
    rclpy.spin(publisher)
    publisher.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()